//  Copyright (C) 2014 Xiande (Triston) Cao

// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// any later version.

// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.

// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef CVSIE_POINT_HPP
#define CVSIE_POINT_HPP


#include<string>

#include "cvsie_datatype.hpp"
#include "cvsie_vector3.hpp"

namespace cvsie {

  template <typename T>
    class CPoint;

  typedef CPoint<double> Point;

  const float POINT_TOLERANCE = 1.0e-16;

  template <typename T>
    class CPoint {
      public:
        T x; // x coordinate
        T y; // y coordinate
        T z; // z coordinate
      public:
        CPoint(T _x = 0, T _y = 0, T _z = 0) : x(_x), y(_y), z(_z) {
        }

        CPoint(const CPoint<T> &p) : x(p.x), y(p.y), z(p.z) {
        }

        inline CPoint<T> & operator=(const CPoint<T> &rhs);
        T GetDistanceToPoint(const CPoint<T> &p) const;

        inline bool operator==(const CPoint<T> &rhs) const;

        // the operator< is for sorting purpose only
        inline bool operator<(const CPoint<T> &rhs) const;

        CVector3<T> &operator-(const CPoint<T> &p) const;

        CPoint<T> &operator+=(const CVector3<T> &vec3);
        CPoint<T> operator+(const CVector3<T> &vec3) const;

        CPoint<T> &operator-=(const CVector3<T> &vec3);
        CPoint<T> operator-(const CVector3<T> &vec3) const;

        void OutputToString(std::string &str);
    };


  template <typename T>
    CPoint<T> & CPoint<T>::operator=(const CPoint<T> &rhs) {
      if (&rhs != this) {
        x = rhs.x;      
        y = rhs.y;      
        z = rhs.z;      
      }
      return *this;
    }


  template <typename T>
    T CPoint<T>::GetDistanceToPoint(const CPoint<T> &p) const {
      T dx = x - p.x;
      T dy = y - p.y;
      T dz = z - p.z;
      return static_cast<T>(sqrt(dx*dx + dy*dy + dz*dz));
    }


  template <typename T>
    bool CPoint<T>::operator==(const CPoint<T> &rhs) const {
      if (GetDistanceToPoint(rhs) < POINT_TOLERANCE) {
        return true;
      }
      return false;
    }


  template <typename T>
    bool CPoint<T>::operator<(const CPoint<T> &rhs) const {
      if ((x < rhs.x) || (x == rhs.x && (y < rhs.y || 
              (y == rhs.y && z < rhs.z)))) { 
        return true;
      }
      return false;
    }


  template <typename T>
    void CPoint<T>::OutputToString(std::string &str) {
      //TODO: xiande.cao@gmail.com
    }


  template <typename T>
  CVector3<T> & CPoint<T>::operator-(const CPoint<T> &p) const {
    T dx = x - p.x;
    T dy = y - p.y;
    T dz = z - p.z;
    return CVector3<T>(dx, dy, dz);
  }


  template <typename T>
  CPoint<T> &CPoint<T>::operator+=(const CVector3<T> &vec3) {
    x += vec3.x;
    y += vec3.y;
    z += vec3.z;
    return *this;
  }

  template <typename T>
  CPoint<T> &CPoint<T>::operator-=(const CVector3<T> &vec3) {
    x -= vec3.x;
    y -= vec3.y;
    z -= vec3.z;
    return *this;
  }

  template <typename T>
  CPoint<T> CPoint<T>::operator+(const CVector3<T> &vec3) const {
    return CPoint<T>(x + vec3.x, y + vec3.y, z + vec3.z);
  }

  template <typename T>
  CPoint<T> CPoint<T>::operator-(const CVector3<T> &vec3) const {
    return CPoint<T>(x - vec3.x, y - vec3.y, z - vec3.z);
  }


  template <typename T>
  CPoint<T> ConvertVector3ToPoint(const CVector3<T> &vec3) {
    return CPoint<T>(vec3.x, vec3.y, vec3.z);
  }

  template <typename T>
  CVector3<T> ConvertPointToVector3(const CPoint<T> &pt) {
    return CVector3<T>(pt.x, pt.y, pt.z);
  }
} // namespace cvsie;

#endif
